#!/usr/bin/env python


import roslib; roslib.load_manifest('intersector')
import rospy
import copy

import intersector.msg
from intersector.msg import Plane
from intersector.msg import Convex_Space



def init():
    output_name = "intersector_input"

    publisher = rospy.Publisher(output_name, Convex_Space)
    rospy.init_node('intersector')

    print "Sleep for 2 seconds to allow the 3d_intersector time to initialize..."
    rospy.Rate(.5).sleep()

    print "Sleep done..."


    while True:
        # this is a horizontal slicer
        space = Convex_Space()

        plane = Plane()
        plane.a = 0
        plane.b = 0
        plane.c = 1
        plane.d = -1

        space.planes.append(plane);

        plane = Plane()
        plane.a =  0
        plane.b =  0
        plane.c = -1
        plane.d =  0

        space.planes.append(plane);

        space.sensor      = "horizontal_slicer"
        space.algorithm   = "horizontal_slicer"
        space.aux_payload = "slicer:horizontal"

        publisher.publish(space)

        rospy.Rate(1).sleep()



if __name__ == '__main__':
    try:
        init()
        rospy.spin()
    except rospy.ROSInterruptException: pass

